#include "Matching.h"




Matching::Matching(void)
{
	//default

	isMatchPointsLoaded=false;

	searchRegionWidth=100;
	searchRegionHeight=100;
	
	correlationWindowWidth=10;
	correlationWindowHeight=10;

	sizeMultplier=1;
	matchPointsNumber=-1;

	rows=3;
	colluns=3;
}

Matching::~Matching(void)
{
}

// et the region of search of points (corners) in an image
void Matching::setSearchRegionSize(int width, int height)
{
	searchRegionWidth=width;
	searchRegionHeight=height;
}


void Matching::setParameters(int rowsgrade, int collunsgrade, int correlationWindowW, int correlationWindowH, int cornersSearchRegionW, int cornersSearchRegionH, int matchPointsNum)
{
   rows=rowsgrade;
   colluns=collunsgrade;
  
   searchRegionWidth=cornersSearchRegionW;
   searchRegionHeight=cornersSearchRegionH;
	
   correlationWindowWidth=correlationWindowW;
   correlationWindowHeight=correlationWindowH;

   matchPointsNumber=matchPointsNum;

}

// Set the window size to do the correlation
void Matching::setCorrelationWindowSize(int width, int height)
{
	correlationWindowWidth=width;
	correlationWindowHeight=height;
}

// set the points of interest in the search region of Imag2
void Matching::setPointsInSearchRegion(CvPoint pointImg1, View &view){

	CvRect rect = cvRect(pointImg1.x-(searchRegionWidth/2),pointImg1.y-(searchRegionHeight/2),searchRegionWidth, searchRegionHeight);
	//printf(" \n point x:%i, y:%i and rect x:%i, y:%i, width:%i, height:%i \n",pointImg1.x, pointImg1.y, rect.x, rect.y, rect.width, rect.height);
    
	view.setInterestPoints(rect);
}



// Search the best match for the point at view1 in view2
Match& Matching::findBestMatch(CvPoint pointImg1, View &view1,View &view2){

		
		double bestvalue=0;
		double	newValue=0;
		CvRect rect;
		int x,y,w,h, bestIndex=0;
		IplImage *imgAux1,*imgAux2;
		Match bestMatch;

	// define the points in the search region on view2
	setPointsInSearchRegion(pointImg1,view2);

	//so para verificar se estava imprimindo o ponto  certo
	//view2.markPoint(view2.interestPoints[0]);
   
	// TESTE APAGAR E RESOLVER
	//view2.interestPointsNumber=1;

	

	if(view1.interestPointsNumber<=0 || view2.interestPointsNumber<=0){
		printf("Please set the interest points (see function(s) available at 'View' class)\n for each View before call findBestMatch \n");
	}else{

	if(    (pointImg1.x-((correlationWindowWidth*sizeMultplier)/2))<=0 
		|| (pointImg1.y-((correlationWindowHeight*sizeMultplier)/2))<=0
		|| (pointImg1.y+((correlationWindowHeight*sizeMultplier)/2))>=view1.imgHeght
		|| (pointImg1.x+((correlationWindowWidth*sizeMultplier)/2))>=view1.imgWidth){
		
			//printf("It is not posible to get a window of this point, please try to change the correlationWindowSize or 'sizeMultplier' variable");
			//printf(" Rect (%i,%i) e (%i,%i)",(int)(pointImg1.x-(correlationWindowWidth*sizeMultplier/2)),(int)(pointImg1.y-(correlationWindowHeight*sizeMultplier/2)),(int)(correlationWindowWidth*sizeMultplier), (int)(correlationWindowHeight*sizeMultplier));
	
			// bestvalue = -1 to indentificate that it was not possible make the corrilation
			bestMatch= Match(cvPoint(-1,-1),cvPoint(-1,-1),-1);
			return bestMatch;

	}
	else{
	

		// Creat the window subimage for correlation IMG1
		rect = cvRect((int)(pointImg1.x-(correlationWindowWidth*sizeMultplier/2)),(int)(pointImg1.y-(correlationWindowHeight*sizeMultplier/2)),(int)(correlationWindowWidth*sizeMultplier), (int)(correlationWindowHeight*sizeMultplier));
	
		// set the region to compare using correlation
	    cvSetImageROI(view1.img, rect);
		
        // Create a subimage with the window size to correlation
		imgAux1 = cvCreateImage(cvGetSize(view1.img),view1.img->depth,view1.img->nChannels);
		
		/* copy subimage */
		cvCopy(view1.img, imgAux1, NULL);

		// Set ROI of the original size of Image
		cvResetImageROI(view1.img);
		
		//GlobalFunctions::DisplayImage("SCR",imgAux1);

		// calcule the correlation of the template with all interest points and store the Max correlation
		for(int i=0; i<view2.interestPointsNumber; i++){
	
			if(correlationWindowWidth<view2.interestPoints[i].x && correlationWindowHeight<view2.interestPoints[i].y){

					//cvsetImageROI(view2.img);

					// Creat the window subimage for correlation IMG2
				    rect = cvRect((int)(view2.interestPoints[i].x-(correlationWindowWidth/2)),(int)(view2.interestPoints[i].y-(correlationWindowHeight/2)),correlationWindowWidth, correlationWindowHeight);
					cvSetImageROI(view2.img, rect);
					imgAux2 = cvCreateImage(cvGetSize(view2.img),view2.img->depth,view2.img->nChannels);
					cvCopy(view2.img, imgAux2, NULL);
					cvResetImageROI(view2.img);
					
				
					//GlobalFunctions::DisplayImage("Template 1",imgAux2);

					// call method do calculate the correlation
					newValue=maxCorrelation(*imgAux1,*imgAux2);
					
					// keep the best values
					if (newValue > bestvalue){
					
						bestvalue=newValue;
						bestIndex=i;
					
					}
			}
	    }
	
	
	// Best Match
	bestMatch= Match(pointImg1,view2.interestPoints[bestIndex],bestvalue);
	bestMatch.matchValue=bestvalue;

	//markMatch(view1,view2,bestMatch);
	
	CvRect rect2 = cvRect(pointImg1.x-(searchRegionWidth/2),pointImg1.y-(searchRegionHeight/2),searchRegionWidth, searchRegionHeight);
	
	// Mark search of region
	//GlobalFunctions::MarkRect(&view2.img,rect2);
	
	}
	
	}
	
	
	return bestMatch;
}

void Matching::MarkSearchRegion(CvPoint point,View &view){

	CvRect rect = cvRect(point.x-(searchRegionWidth/2),point.y-(searchRegionHeight/2),searchRegionWidth, searchRegionHeight);
	
	// Mark search of region
	GlobalFunctions::MarkRect(&view.img,rect);

}


void Matching::RefineMatching(int ntimes){

	int point_count;
	int counter=0;
	int s;
	CvMat* points1;
	CvMat* points2;
	CvMat* status;
	CvMat* fundamental_matrix;
	
	
	// Run ntimes times
	for(int x=0;x<ntimes;x++){
	
	
	point_count = matchPointsNumber;

	
	points1 = cvCreateMat(2,point_count,CV_32FC1);
	points2 = cvCreateMat(2,point_count,CV_32FC1);
	status = cvCreateMat(1,point_count,CV_8UC1);



	/* Fill the points here ... */
	for( int i = 0; i < point_count; i++ )
	{
		cvmSet(points1,0,i,matchPointsScene1[i].x);
		cvmSet(points1,1,i,matchPointsScene1[i].y);

		cvmSet(points2,0,i,matchPointsScene2[i].x);
		cvmSet(points2,1,i,matchPointsScene2[i].y);
		
		/*points1->data.fl[i*2] = matchPointsScene1[i].x; 
		points1->data.fl[i*2+1] = matchPointsScene1[i].y;
		points2->data.fl[i*2] = matchPointsScene2[i].x; 
		points2->data.fl[i*2+1] = matchPointsScene2[i].y;*/
	}

	fundamental_matrix = cvCreateMat(3,3,CV_32FC1);

	int fm_count = cvFindFundamentalMat( points1, points2,fundamental_matrix,CV_FM_RANSAC,1.0,0.999,status);


	for(int i =0; i<matchPointsNumber; i++){
	
		s=status->data.fl[i];
		//cvmGet(status,0,i);		
	
		if(s==0){
			
			
			// erase the (i+1)th element
			matchPointsScene1[i].x=-1;
			matchPointsScene1[i].y=-1;

			matchPointsScene2[i].x=-1;
			matchPointsScene2[i].y=-1;
			
			
			//matchPointsScene1.erase (matchPointsScene1.begin()+(i));
			//matchPointsScene2.erase (matchPointsScene2.begin()+(i));
			counter++;
		}
		
	
	}

	
	int pos=0;
	
	while(pos<matchPointsScene1.size()){

		
	
		if(matchPointsScene1[pos].x==-1){
		
			matchPointsScene1.erase (matchPointsScene1.begin()+(pos));
			matchPointsScene2.erase (matchPointsScene2.begin()+(pos));

		}
		else{
		
			pos++;

		}

		

	}
	
	matchPointsNumber=matchPointsScene1.size();	
	cout<< counter <<" outliers \n ";
	
	}


}



void Matching::drawLines(View &view1){

	
	if(isMatchPointsLoaded){
		
		for(int i=0;i<matchPointsNumber;i++){

			cvLine(view1.img,matchPointsScene1[i], matchPointsScene2[i], cvScalar(0,255,0), 1);
		}
	
		GlobalFunctions::DisplayImage("Lines",view1.img);
		GlobalFunctions::HoldImages();
	
	}
	else {
	
		cout<< "no match Points loaded, call method findAllBestMatches";

	}



}



void Matching::findAllBestMatches(View &view1,View &view2,int row, int col, int numpoints){
	
	
	isMatchPointsLoaded=true;

	// define the heght of the "table cell"
	int heighwindow=(view1.img)->height/row;
	// define the width of the "table cell"
	int widthwindow=(view1.img)->width/col;
	matchPointsNumber=0;
	int niterations;
    const Match *ma;

	CvRect rect;

	int x=0;
	int y=0;

	// clear the vectors os correspondence points
	matchPointsScene1.clear();
	matchPointsScene2.clear();

	for (int i=0;i<row;i++){
		
		x=0; // fisrt position of the line 
		
		for (int j=0;j<col;j++){
			
			
			rect = cvRect(x,y,widthwindow,heighwindow);
			
			// printf(" rect (%i,%i,%i,%i) \n",x,y,widthwindow,heighwindow);
			
			
			 x+=widthwindow; // runs the line

			
			 
			 // set the interest points inside teh region rect
			 view1.setInterestPoints(rect);
				
			 niterations=numpoints;
			 // case of no points enought in the search area
			 if(numpoints>view1.interestPointsNumber) {
				 niterations=view1.interestPointsNumber;
			 }
			 
			 
			 for(int k=0;k<niterations;k++){

				
				 // find each match
				 Match testM=findBestMatch(view1.interestPoints[k],view1,view2);
				 ma= new Match(testM);

				 // verify if the match was done correctly
				 if(ma->matchValue!=-1){
					 
					 //fill the vectors of points correspondence
					matchPointsScene1.push_back(ma->point1);
					matchPointsScene2.push_back(ma->point2);
					
					// stores the number of matchs
					matchPointsNumber++;

				 }
				 else{
					 printf("pas possible");
				 }
				 
					 //findBestMatch(view1.interestPoints[k],view1,view2);
				
				// printf(" poins %i,%i  e %i,%i \n ",ma->point1.x,ma->point1.y,ma->point2.x,ma->point2.y);
				 
				 
				 

			 }
			 
			 // draw the rect for each cell over image
			GlobalFunctions::MarkRect(&view1.img,rect);

			 
		}
		y+=heighwindow; // runs te colunn
	}

	
	ma->~Match();
	
	


}


// set the points of interest in the search region of Imag2
double Matching::maxCorrelation(IplImage &img, IplImage &tpl){


	int iwidth = img.width - tpl.width + 1;
	int iheight = img.height -  tpl.height + 1;
	
	
	// Create a subimage with the window size to correlation
	IplImage *res = cvCreateImage( cvSize( iwidth, iheight ), 32, 1 );
	
	/* perform template matching */
	cvMatchTemplate(&img, &tpl, res, 5);

	
	//GlobalFunctions::DisplayImage("Correlation output",res);
	
	/* find best matches location */
	CvPoint minloc, maxloc;
	double minval, maxval;
	cvMinMaxLoc(res, &minval, &maxval, &minloc, &maxloc, 0);
	

	return maxval;
}


// Mark the match points in the 2 Scenes
void Matching::markMatch(View &view1, View &view2, Match &match)
{
	
		CvPoint Point1[1];
		CvPoint Point2[1];
		Point1[0]=match.point1;
		Point2[0]=match.point2;
		
		GlobalFunctions::MarkCorners(Point1,&view1.img,1);
		GlobalFunctions::MarkCorners(Point2,&view2.img,1);

}


// Mark the match points in the 2 Scenes
void Matching::markMatchsWithNumbers(View &view1, View &view2, int numberPoints)
{
	
	if(matchPointsNumber>0){

	    GlobalFunctions::MarkCornersWithNumbers(matchPointsScene1,&view1.img,numberPoints); 
		GlobalFunctions::MarkCornersWithNumbers(matchPointsScene2,&view2.img,numberPoints); 
	}
	else{
		cout<<"call the method findallbestmatchs before calling this method";
	}

}



